#include "DribbleBehaviour.h"
#include "../CommonFunctions.h"

// Zoznam informacii o hracoch, ktorych vidi hrac
extern CEvaluatingPlayerList* ePlayerList;

//konstruktor
DribbleBehaviour::DribbleBehaviour()
{
	eActualNeckDirection = LEFT;
	ePreviousNeckDirection = FRONT;
}

/*
 * Driblovanie. Podla povodneho komentara funkcia, ktorej by ste sa mali vyhnut:)
 * 
 * @param position nepouzity parameter...
 * @param angle uhol, do ktoreho sa ma driblovat s loptou
 * @param bFast ci hrac s loptou bezi
 *
 */
void DribbleBehaviour::Behave(const Point& position, float angle,  bool bFast)
{
	SKILLS_DBGN("%i: ->dribbling (2) to position", world->state->GetSimTime());
	SKILLS_DBGN("%i: ->position (%3.2f, %3.2f) angle %3.2f", world->state->GetSimTime(), position.GetX(), position.GetY(), angle);
 
	if(angle > 90)
	{
		angle = 90;
	}
	else if(angle < -90)
	{
		angle = -90;
	}

	Point my_nextpos = world->me->pos + world->me->vel;
	Point ball_nextpos = world->ball->pos + world->ball->vel;

	float uhol = CalcDir(position - my_nextpos, world->me->GetAbsDir()).GetDeg();

	// DASHPOWER MANAZMENT:
	// --------------------
	float dashpower;
		
	// Bezi sa co narychlejsie, ak driblujeme hlboko v obrane supera alebo nasej obrane
	if(IsPointInRectArea(world->ball->pos.GetX(), world->ball->pos.GetY(), 32.0, -34.0, 52.5, 34.0) ||
		IsPointInRectArea(world->ball->pos.GetX(), world->ball->pos.GetY(), -52.5, -34.0, -25.0, 34.0))
	{
		
		dashpower = GetRunToPositionBehaviour()->RecommendDashToMaxSpeed();
	}
	else if(ePlayerList->NoEnemiesAround(world->me->pos))
	{
		dashpower = GetRunToPositionBehaviour()->RecommendDashToMaxSpeed();
	}	
	else
		dashpower = GetRunToPositionBehaviour()->RecommDashPower(true);

	// --------------------

	const float acc = min(world->params->server->player_accel_max, dashpower * world->me->type->dash_power_rate * world->me->GetEffort());

	const float karea = world->params->server->ball_size + world->me->type->player_size + world->me->type->kickable_margin;
	const float controll_dist = world->params->server->ball_size + world->me->type->player_size + 0.45f * world->me->type->kickable_margin;

	// Musim zmenit smer
	if(fabsf(uhol) > 10) 
	{
		if((ball_nextpos - my_nextpos).GetLength() < karea) // mozem sa otocit, lopta daleko neujde
		{
			float moment = GetRunToPositionBehaviour()->CompMoment(uhol);
			commandGenerator->Turn(moment);
			GetTurnNeckToBehaviour()->Behave(ball_nextpos,my_nextpos, GetTurnToBehaviour()->CompTurn(moment));
		}

		// Musim loptu spracovat, aby neutiekla
		else 
		{
			Point my_targetpos = world->me->pos + world->me->vel * (1 + world->me->type->player_decay);
			float dir = (world->me->GetAbsDir() + DegAngle(uhol + angle)).GetRad();
			Point ciel = my_targetpos + RectVector(controll_dist * cosf(dir), controll_dist * sinf(dir));
			ball_nextpos = world->ball->pos + (ciel - world->ball->pos) / (1 + world->params->server->ball_decay);
			GetNearKickBehaviour()->Behave(ball_nextpos);

			Point pointLookDirection = ball_nextpos;

			if(world->me->pos.GetX() < 33)
			{
				TransformLookDirection(pointLookDirection);
			}
			
			GetTurnNeckToBehaviour()->Behave(pointLookDirection, my_nextpos, 0);
			
			if(world->me->pos.GetX() < 33)
			{
				GetTurnNeckToBehaviour()->NeckSynchronizationWithVI(NEW_VI);
			}
		}
	}
	// Idem s loptou rovno
	else 
	{
		// Ak sa nemozno pohybovat
		if(!DribbleRun(angle))
		{
			float dir = (world->me->GetAbsDir() + DegAngle(angle)).GetRad();
			uhol = world->me->GetAbsDir().GetRad();
			Point my_targetpos = world->me->pos + world->me->vel * (1 + world->me->type->player_decay) + RectVector(acc * cosf(uhol), acc * sinf(uhol));
			Point ciel = my_targetpos + RectVector(controll_dist * cosf(dir), controll_dist * sinf(dir));
			
			if(bFast == true)
			{
				ball_nextpos = world->ball->pos + (ciel - world->ball->pos) * 1.2f / (1 + world->params->server->ball_decay);
			}
			else
			{
				ball_nextpos = world->ball->pos + (ciel - world->ball->pos) * 1.05f / (1 + world->params->server->ball_decay);
			}
			
			GetNearKickBehaviour()->Behave(ball_nextpos);

			Point pointLookDirection = ball_nextpos;

			if(world->me->pos.GetX() < 33)
			{
				TransformLookDirection(pointLookDirection);
			}
			
			GetTurnNeckToBehaviour()->Behave(pointLookDirection, my_nextpos, 0);
			
			if(world->me->pos.GetX() < 33)
			{
				GetTurnNeckToBehaviour()->NeckSynchronizationWithVI(NEW_VI);
			}	
		}
	}
}

/*
 * Pomocna funkcia pre driblovanie. Podla povodneho komentara neprehladna a nedoporucena pouzivat.
 *
 * @param angle uhol, do ktoreho sa ma driblovat
 *
 * @return doporucenie ci sa ma driblovat
 */
bool DribbleBehaviour::DribbleRun(float angle)
{
	SKILLS_DBGN("%i: ->dribble run", world->state->GetSimTime());

	const float karea = world->params->server->ball_size + world->me->type->player_size + world->me->type->kickable_margin;
	
	// POZN.: Nepresny (resp. hruby) vypocet dalsej pozicie lopty. (chyba vo vzorci ball_decay)
	const Point ball_nextpos = world->ball->pos + world->ball->vel;

	// Loptu nema sancu dostihnut za ziadnu cenu
	if((ball_nextpos - world->me->pos).GetLength() > (world->me->type->player_speed_max + karea))
		return false;
	
	const Point A = world->me->pos + world->me->vel;
	const float dashpower = GetRunToPositionBehaviour()->RecommDashPower(true);
	const float maxacc = dashpower * world->me->type->dash_power_rate * world->me->GetEffort();
	float u = world->me->GetAbsDir().GetRad();

	const Point B = A + RectVector(maxacc * cosf(u), maxacc * sinf(u));

	// Vzdialenost bodu od priamky
	float sDS =  ((B.GetX() - A.GetX())*(ball_nextpos.GetY() - A.GetY()) - (B.GetY() - A.GetY())*(ball_nextpos.GetX() - A.GetX())) / maxacc;
	float DS = fabsf(sDS);

	if(DS > (0.9f * karea))
	{
		// Na loptu nedociahne, resp. riziko, ze utecie
		return false;
	}

	const float controll_dist = world->params->server->ball_size + world->me->type->player_size + 0.45f * world->me->type->kickable_margin;
	
	if(fabsf(sDS - controll_dist * sinf(angle*M_PI/180)) > 0.4f)
	{
		return false;
	}

	float T = ( (B.GetX() - A.GetX())*(ball_nextpos.GetX() - A.GetX()) + (B.GetY() - A.GetY())*(ball_nextpos.GetY() - A.GetY()) ) / (maxacc*maxacc);

	if(T > 1)
	{
		if( (ball_nextpos - B).GetLength() > karea)
		{
			// Lopta je prilis daleko
			return false;
		}
	}
	else if(T <= 0)
	{
		// Lopta je prilis daleko alebo sa absolutne neoplati pohnut
		return false;
	}

	// Ak je prilis blizko
	if(DS < controll_dist )
	{
		float dt = sqrtf( controll_dist*controll_dist - DS*DS )/ maxacc;
		T -= dt;
	}

	if(T > 1)
	{
		T = 1;
	}
	else if(T <= 0)
	{
		return false;
	}

	if(T > 0.5f)
	{
		float power = T * dashpower;
		commandGenerator->Dash(power);
		GetTurnNeckToBehaviour()->Behave(ball_nextpos, world->me->pos + GetRunToPositionBehaviour()->PredictNextVelocity(power),0);
		return true;
	}
	
	// Bezalo by sa prilis pomaly, neoplati sa bezat
	else 
	{
		return false;
	}
}

/*
 * Sluzi na transformovanie pohladu hraca, tak aby sa pribezne obzeral vlavo, dopredu a vpravo
 *
 * @param pointDirection bod, do ktoreho sa dribluje
 */
void DribbleBehaviour::TransformLookDirection(Point& pointDirection)
{
	// Vypocitam bod, ktory bude priamo pred natocenim tela v polomere rovnom dlzke hrac <-> povodny bod pohladu
	double lfRadius = (pointDirection - world->me->pos).GetLength();
	Point tmpPoint(world->me->pos.GetX() + (float)lfRadius, world->me->pos.GetY());
	pointDirection.SetX(world->me->pos.GetX() + cos(world->me->GetAbsDir().GetRad()) * (float)lfRadius);
	pointDirection.SetY(world->me->pos.GetY() + sin(world->me->GetAbsDir().GetRad()) * (float)lfRadius);

	bool bNeckDirectionCorrect;
	double lfX, lfY;
	int nLookFailedCount = 0;

	do
	{
		bNeckDirectionCorrect = true;

		if((ePreviousNeckDirection == FRONT) && (eActualNeckDirection == LEFT))
		{
			NeckDirection tmpNeckDirection = eActualNeckDirection;
			eActualNeckDirection = ePreviousNeckDirection;
			ePreviousNeckDirection = tmpNeckDirection;
		}
		else if((ePreviousNeckDirection == LEFT) && (eActualNeckDirection == FRONT))
		{
			NeckDirection tmpNeckDirection = eActualNeckDirection;
			eActualNeckDirection = RIGHT;
			ePreviousNeckDirection = tmpNeckDirection;
		}
		else if((ePreviousNeckDirection == FRONT) && (eActualNeckDirection == RIGHT))
		{
			NeckDirection tmpNeckDirection = eActualNeckDirection;
			eActualNeckDirection = ePreviousNeckDirection;
			ePreviousNeckDirection = tmpNeckDirection;
		}
		else if((ePreviousNeckDirection == RIGHT) && (eActualNeckDirection == FRONT))
		{
			NeckDirection tmpNeckDirection = eActualNeckDirection;
			eActualNeckDirection = LEFT;
			ePreviousNeckDirection = tmpNeckDirection;
		}

		double lfDeltaRadAngle;

		if(eActualNeckDirection == LEFT)
		{
			lfDeltaRadAngle = (-world->me->GetNeckDir().GetDeg() - 80) * pi / 180;
		}
		else if(eActualNeckDirection == FRONT)
		{
			lfDeltaRadAngle = -world->me->GetNeckDir().GetRad();
		}
		else if(eActualNeckDirection == RIGHT)
		{
			lfDeltaRadAngle = (-world->me->GetNeckDir().GetDeg() + 80) * pi / 180;
		}

		lfX = pointDirection.GetX();
		lfY = pointDirection.GetY();

		RotatePoint(world->me->pos.GetX(), world->me->pos.GetY(), lfX, lfY, lfDeltaRadAngle);

		Point nextLookAtTarget((float)lfX, (float)lfY);
		Vector tmpVect = nextLookAtTarget - world->me->pos; 
		tmpVect /= tmpVect.GetLength();	// Jednotkovy vektor  

		// Optimalizacia pohladov - niektore pohlady pri okrajoch ihriska neprinesu informacnu hodnotu s VI
		// Pri okrajoch ihriska nemaju niektore pohlady zmysel

		Point myPosition = world->me->pos;

		// Horny okraj ihriska
		if(myPosition.GetY() < -24)
		{
			tmpVect *= 18;
			if((myPosition + tmpVect).GetY() <= -34)
				bNeckDirectionCorrect = false;
		}
		else if(myPosition.GetY() > 24)
		{
			tmpVect *= 18;
			if((myPosition + tmpVect).GetY() >= 34)
				bNeckDirectionCorrect = false;
		}

		// Prevencia proti zacykleniu, max. 3 neuspesne prechody
		if(++nLookFailedCount > 3)
			bNeckDirectionCorrect = true;

	} while(bNeckDirectionCorrect != true);

	if(eActualNeckDirection == FRONT)
		DATA_STORAGE.turnNeck.avoidNextCorrection = true;
	else
		DATA_STORAGE.turnNeck.avoidNextCorrection = false;
	
	pointDirection.SetX((float)lfX);
	pointDirection.SetY((float)lfY);
}

/*
 * Tato metoda riadi vypocitavanie optimalneho driblovacieho bodu
 *
 * @return stale vracia true, otazkou je ci je potrebna navratova hodnota
 */
bool DribbleBehaviour::ManageDribblePoint()
{
	bool bResult = true;
	static int nLastDribblePointCalculationCycle;
	int nActualCycle = world->state->GetSimTime();
	
//	if(nActualCycle - 2 > nLastDribblePointCalculationCycle)
//	{
		DATA_STORAGE.lfDribbleX = world->me->pos.GetX();
		DATA_STORAGE.lfDribbleY = world->me->pos.GetY();

		bResult = ePlayerList->CalculateOptimalDribblePointAux(DATA_STORAGE.lfDribbleX, DATA_STORAGE.lfDribbleY, 8.0);
		nLastDribblePointCalculationCycle = nActualCycle;
		
//	}
	return bResult;		
}

/*
 * driblovanie k urcitemu bodu
 *
 * @param pos pozicia kam sa ma driblovat
 *
 * @return Vrati true a hrac dribluje k bodu, ak super nemoze kopnut do lopty. Inak vrati false.
 */
bool DribbleBehaviour::SmartDribleTo(const Point& pos)
{
	int who;
	const float karea = world->variables->GetDribleSurroundingsMinWidth();
	for (who = 0; who < world->enemies.GetSize(); who++)
	{
		// protihrac uz moze kopnut do lopty
		if ((world->ball->pos - world->enemies[who].pos).GetLength() <= karea * 1.1f)
			return false;
	}

	const float dir = (pos - world->me->pos).GetAngle().GetDeg();
	const float controll_dist = world->params->server->ball_size + world->me->type->player_size + 0.45f * world->me->type->kickable_margin;

	float bestdir = 0;
	float maxvaha = 0;
	float vaha;

	for (float reldir = -90; reldir <= 90; reldir += 5)
	{
		float uhol = (dir + reldir) * M_PI/180;

		Point driblepos = world->me->pos + RectVector(cosf(uhol)* controll_dist,sinf(uhol) * controll_dist);

		vaha = 1000; // velke cislo
		for(who = 0; who < world->enemies.GetSize(); who++)
		{
			if((world->enemies[who].pos - world->me->pos).GetLength() > world->variables->GetDribleSurroundingsMaxWidth())
				continue;

			const float vzdial = (world->enemies[who].pos - driblepos).GetLength();
			
			if (vzdial <= karea)
			{
				vaha = 0;
				break;
			}
			if (!InTheMiddle(world->me->pos, driblepos, world->enemies[who].pos))
			{
				if (vzdial <= (karea + world->params->server->player_speed_max))
				{
					vaha = 0;
					break;
				}
				vaha = min(vaha, vzdial - (karea + world->params->server->player_speed_max));
			}
			else
			{
				vaha = min(vaha, vzdial - karea);
			}

		}

		if(vaha > maxvaha)
		{
			maxvaha = vaha;
			bestdir = reldir;
		}

	}

	if(maxvaha > 0)
	{
		Behave(pos,bestdir);
		//DBGN("%f",bestdir);
		return true;
	}
	return false;
}
